//Copyright 2012 Trevin Liberty
//GNU GPL v3
//Author: Trevin Liberty

#include "AStar.h"

#include "../Entity.h"

AStar::AStar(Map* map) {
	queueIn = new Thread_Queue_In();
	queueOut = new Thread_Queue_Out();

	queueIn->queue = new list<PathingQueueEntry*>();
	queueOut->queue = new vector<CompletedPath*>();

	queueIn->mutex = al_create_mutex();
	queueOut->mutex = al_create_mutex();

	queueIn->cond = al_create_cond();
	queueOut->cond = al_create_cond();

	this->map = map;

	//for(int i = 0; i < NUM_THREADS; i++) {
		/*thread = al_create_thread(Pathing_Thread, this);
		al_start_thread(thread);*/
	//}
}

//g(n) = the cost of getting from the initial node to n.
//h(n) = the estimate, according to the heuristic function, of the cost of getting from n to the goal node.
//f(n) = g(n) + h(n); intuitively, this is the estimate of the best solution that goes through n.

bool sortFunc(Node* n1, Node* n2) {
	return n1->f > n2->f;
}

list<Node*>::iterator contains(list<Node*>* searchList, Node* n) {
	list<Node*>::iterator it;
	for(it = searchList->begin(); it != searchList->end(); ++it) {
		if(*(*it) == *n)
			return it;
	}
	return searchList->end();
}

vector<Node*>::iterator contains(vector<Node*>* searchList, Node* n) {
	vector<Node*>::iterator it;
	for(it = searchList->begin(); it != searchList->end(); ++it) {
		if(*(*it) == *n)
			return it;
	}
	return searchList->end();
}

list<Node*>* AStar::getNeighbors(const Node* node) {
	std::list<Node*>* neighbors = new list<Node*>();
	Node* n;
	int x = node->pos.x;
	int y = node->pos.y;

	for(int i = -1; i < 2; i++) {
		for(int j = -1; j < 2; j++) {
			if(x+i < 0 || y+j < 0 || x+i >= map->numTilesX || y+j >= map->numTilesY)
				continue;
			if((i == 0 && j == 0) || !map->mapArray[x+i][y+j]->walkable)
				continue;
			n = new Node();
			if(map->mapArray[x+i][y+j]->doohickey != NULL)
				n->isOccupied = true;

			n->pos.x = x+i;
			n->pos.y = y+j;
			n->g = map->mapArray[x+i][y+j]->cost;
			neighbors->push_back(n);
		}
	}

	return neighbors;
}

float AStar::h(Node* cNode, Node* gNode) {
	return max(abs(cNode->pos.x - gNode->pos.x), abs(cNode->pos.y - gNode->pos.y));
	//return std::abs(cNode->pos.x - gNode->pos.x)+std::abs(cNode->pos.y - gNode->pos.y);
}

float AStar::g(Node* currentNode, Node* neighborNode) {
	int x = neighborNode->pos.x - currentNode->pos.x;
	int y = neighborNode->pos.y - currentNode->pos.y;

	int k = neighborNode->g;
	if(currentNode->isOccupied)
		k = 5;

	if(x != 0 && y != 0){
		return sqrt(2.0) + k;
	}
	return 1.0 + k;
}

void AStar::deleteVector(vector<Node*>* vec) {
	vector<Node*>::iterator it;
	for(it = vec->begin(); it != vec->end(); ++it)
		delete (*it);
}

void AStar::deleteList(list<Node*>* lis) {
	list<Node*>::iterator it;
	for(it = lis->begin(); it != lis->end(); ++it)
		delete (*it);
}

list<Point<int>>* AStar::getPath(Point<int> start, Point<int> goal, ALLEGRO_THREAD *thr) {
	list<Point<int>>* currentPath = new list<Point<int>>();
	
	vector<Node*>* openList = new vector<Node*>();
	make_heap(openList->begin(), openList->end());

	list<Node*>* closedList = new list<Node*>();
	list<Node*>* neighbors = new list<Node*>();

	Node* startNode = new Node(start);
	Node* goalNode = new Node(goal);

	list<Node*>::iterator it;
	vector<Node*>::iterator itOpen;
	list<Node*>::iterator itClosed;
	int x = goal.x;
	int y = goal.y;
	if(!map->mapArray[x][y]->walkable)
		return currentPath;

	x = start.x;
	y = start.y;
	if(!map->mapArray[x][y]->walkable)
		return currentPath;

	startNode->f = h(startNode, goalNode);
	startNode->g = 0;

	openList->push_back(startNode);
	push_heap(openList->begin(), openList->end(), sortFunc);

	Node* current = new Node();
	Node* neighbor = new Node();

	bool atGoal = false;

	double time = al_get_time();

	while(!openList->empty()) {
		current = openList->front();
		pop_heap(openList->begin(), openList->end(), sortFunc);
		openList->pop_back();

		if(current->pos.x == goalNode->pos.x && current->pos.y == goalNode->pos.y) {
			atGoal = true;
			break;
		}

		if((al_get_time() - time) > 10) {
			break;
		}

		if(al_get_thread_should_stop(thr))
			return NULL;

		closedList->push_front(current);

		neighbors = getNeighbors(current);

		for(it = neighbors->begin(); it != neighbors->end();) {
			++it;
			neighbor = neighbors->front();
			neighbors->pop_front();

			neighbor->g = current->g + g(current, neighbor);
			neighbor->f = neighbor->g + h(neighbor, goalNode);

			itOpen = contains(openList, neighbor);
			if(itOpen != openList->end() &&  neighbor->f >= (*itOpen)->f)
				continue;

			itClosed = contains(closedList, neighbor);
			if(itClosed != closedList->end() && neighbor->f >= (*itClosed)->f)
				continue;

			openList->erase(itOpen, itOpen);
			make_heap(openList->begin(), openList->end(), sortFunc);

			closedList->erase(itClosed, itClosed);
			neighbor->parent = current;

			openList->push_back(neighbor);
			push_heap(openList->begin(), openList->end(), sortFunc);
		}
	}

	//cout << al_get_time() - time << endl << endl;

	if(atGoal)
		currentPath->push_front(goal);

	while(current != NULL) {
		currentPath->push_front(Point<int>(current->pos.x, current->pos.y));
		current = current->parent;
	}

	deleteVector(openList);
	deleteList(closedList);
	deleteList(neighbors);

	delete openList;
	delete closedList;
	delete neighbors;

	return currentPath;
}

bool AStar::isEmptyQueueIn() {
	al_lock_mutex(queueIn->mutex);
	bool t = queueIn->queue->empty();
	al_unlock_mutex(queueIn->mutex);
	return t;
}

bool AStar::isEmptyQueueOut() {
	al_lock_mutex(queueOut->mutex);
	bool t = queueOut->queue->empty();
	al_unlock_mutex(queueOut->mutex);
	return t;
}

PathingQueueEntry* AStar::queueInGet() {
	PathingQueueEntry *p = NULL;

	al_lock_mutex(queueIn->mutex);
	if(queueIn->queue->empty()) {
		al_unlock_mutex(queueIn->mutex);
		return NULL;
	}
	p = queueIn->queue->front();
	queueIn->queue->pop_front();
	al_unlock_mutex(queueIn->mutex);

	return p;
}

void AStar::addEntry(PathingQueueEntry* entry) {
	list<PathingQueueEntry*>::iterator it;
	al_lock_mutex(queueIn->mutex);
	for(it = queueIn->queue->begin(); it != queueIn->queue->end(); ++it) {
		if((*it)->e->getID() == entry->e->getID()) {
			queueIn->queue->erase(it);
			break;
		}
	}
	queueIn->queue->push_back(entry);
	al_unlock_mutex(queueIn->mutex);
}

CompletedPath* AStar::queueOutGet(Entity* e) {
	CompletedPath* path = NULL;

	al_lock_mutex(queueOut->mutex);
	if(queueOut->queue->empty()) {
		al_unlock_mutex(queueOut->mutex);
		return NULL;
	}
	for(unsigned int i = 0; i < queueOut->queue->size(); ++i) {
		if(queueOut->queue->at(i)->e->getID() == e->getID()) {
			path = queueOut->queue->at(i);
			queueOut->queue->erase(queueOut->queue->begin()+i);
			break;
		}
	}
	al_unlock_mutex(queueOut->mutex);

	return path;
}

void AStar::removeEntityPaths(Entity* e) {
	al_lock_mutex(queueOut->mutex);
	for(unsigned int i = 0; i < queueOut->queue->size(); ++i) {
		if(queueOut->queue->at(i)->e->getID() == e->getID()) {
			queueOut->queue->erase(queueOut->queue->begin()+i);
			break;
		}
	}
	al_unlock_mutex(queueOut->mutex);

	list<PathingQueueEntry*>::iterator it;
	al_lock_mutex(queueIn->mutex);
	for(it = queueIn->queue->begin(); it != queueIn->queue->end(); ++it) {
		if((*it)->e->getID() == e->getID()) {
			queueIn->queue->erase(it);
			break;
		}
	}
	al_unlock_mutex(queueIn->mutex);
}

void AStar::addCompletedPath(CompletedPath* path) {
	al_lock_mutex(queueOut->mutex);
	queueOut->queue->push_back(path);
	al_unlock_mutex(queueOut->mutex);
}

AStar::~AStar() {
	al_destroy_mutex(queueIn->mutex);
	al_destroy_mutex(queueOut->mutex);
	al_destroy_cond(queueIn->cond);
	al_destroy_cond(queueOut->cond);

	list<PathingQueueEntry*>::iterator it;
	vector<CompletedPath*>::iterator itc;

	for(it = queueIn->queue->begin(); it != queueIn->queue->end();) {
		delete (*it);
		it = queueIn->queue->erase(it);
	}

	for(itc = queueOut->queue->begin(); itc != queueOut->queue->end();) {
		delete (*itc);
		itc = queueOut->queue->erase(itc);
	}
}